import json
import math

import numpy as np
import pandas as pd
from matplotlib import pyplot as plt

folder_path= "UAV-HAV-5-2-25-5--3model-1"
file_name="datasource.csv"
df=pd.read_csv(f'{folder_path}/{file_name}')
fig1 = plt.figure()
vehicle_names = ["000", "001"]
ax = fig1.add_subplot(111, projection='3d')
ax.plot([json.loads(u)[0] for u in df["000"].tolist()],
            [json.loads(u)[1] for u in df["000"].tolist()],
            [json.loads(u)[2] for u in df["000"].tolist()], label="A",color='black')
ax.plot([json.loads(u)[0] for u in df["001"].tolist()],
            [json.loads(u)[1] for u in df["001"].tolist()],
            [json.loads(u)[2] for u in df["001"].tolist()], label="B",color='black', linestyle='--')
ax.legend()
ax.set_zlim(0, 40)
# 设置坐标轴标签
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('uav_positions')
ax.set_aspect('equal')
x = np.arange(0, len(df))
fig1, axs1 = plt.subplots(4, 1)
axs1[0].plot(x, df["radius"].tolist(), label="Rc",color='black')
axs1[0].plot(x, df["distance"].tolist(), label="distance",color='black', linestyle='--')
axs1[0].plot(x, df["neighbor_dist"].tolist(), label="h",color='black', linestyle='-.')
axs1[0].legend()
axs1[1].plot(x, [json.loads(u)[0] for u in df["000_velocity"].tolist()], label="A_x_velocity",color='black')
axs1[1].legend()
axs1[2].plot(x, [json.loads(u)[1] for u in df["000_velocity"].tolist()], label="A_y_velocity",color='black')
axs1[2].legend()
axs1[3].plot(x, [json.loads(u)[2] for u in df["000_velocity"].tolist()], label="A_z_velocity",color='black')
axs1[3].legend()

fig2, axs2 = plt.subplots(4, 1)
axs2[0].plot(x, df["radius"].tolist(), label="Rc",color='black')
axs2[0].plot(x, df["distance"].tolist(), label="distance",color='black', linestyle='--')
axs2[0].plot(x, df["neighbor_dist"].tolist(), label="h",color='black', linestyle='-.')
axs2[0].legend()
axs2[1].plot(x, [-json.loads(u)[0] for u in df["001_velocity"].tolist()], label="B_x_velocity",color='black')
axs2[1].legend()
axs2[2].plot(x, [-json.loads(u)[1] for u in df["001_velocity"].tolist()], label="B_y_velocity",color='black')
axs2[2].legend()
axs2[3].plot(x, [json.loads(u)[2] for u in df["001_velocity"].tolist()], label="B_z_velocity",color='black')
axs2[3].legend()
# 调整子图间距
plt.tight_layout()
# 显示图形
plt.show()
